package sc;

import java.awt.Color;
import java.awt.Graphics;
import java.awt.Polygon;
import java.util.ArrayList;
import java.util.List;

import sc.math.CardinalCurve;
import sc.math.Spline2D;
import sc.math.Vector2D;

/**
 * Racetrack2D provides methods to create and manage a C1-continuous closed
 * spline. The intention, as the name suggests, is that this represents a
 * racetrack. 
 */
public final class Racetrack2D {

	// The tracks spline
	private final CardinalCurve splines_curve = new CardinalCurve();
	
	private final Spline2D spline = new Spline2D(this.splines_curve);

	// Half width of track
	private double halfwidth;

	// Number of intervals between successive control points
	private double interval;

	// Intervals of middle curve
	private List<double[]> midcurve;

    // Intervals of left curve
	private List<double[]> leftcurve;

    // Intervals of right curve
	private List<double[]> rightcurve;

	// List of polygons that make up the track
	private final List<Polygon> polygons = new ArrayList<Polygon>();

	/** Creates an instance of Racetrack2D */
	public Racetrack2D() {
		// Default attributes of Racetrack
		setInterval(10);
		setWidth(10);
		setTension(-0.5);
	}

	/**
	 * Sets the spline interval of the race track.
	 *
	 * @param interval
	 * 			Number of intervals between successive control points
	 */
	public void setInterval(double interval) {
		this.interval = interval;
	}

	/**
	 * Gets the spline interval of the race track.
	 *
	 * @return Number of intervals between successive control points
	 */
	public double getInterval() {
		return this.interval;
	}
	
	/**
	 * Sets the width of the race track.
	 *
	 * @param width
	 * 			Width of track
	 */
	public void setWidth(double width) {
		this.halfwidth = width * 0.5;
	}
	
	/**
	 * Gets the width of the race track.
	 *
	 * @return Width of track
	 */
	public double getWidth() {
		return this.halfwidth * 2;
	}
	
	/**
	 * Sets the tension of the race tracks spline.
	 *
	 * @param tension
	 * 			New tension value
	 */
	public void setTension(double tension) {
		this.splines_curve.setTension(tension);
	}
	
	/**
	 * Gets the tension of the race tracks spline.
	 *
	 * @return Tension value
	 */
	public double getTension() {
		return this.splines_curve.getTension();
	}
	
	/**
	 * Gets the tracks Spline.
	 * 
	 * @return the spline
	 */
	public Spline2D getSpline() {
		return this.spline;
	}

	/**
	 * Appends a new control point.
	 *
	 * @param x
	 * 			x-coordinate of new control point
	 * @param y
	 * 			y-coordinate of new control point
	 */
	public void appendControlPoint(double x, double y) {
		if (sanitise(x, y))
			this.spline.points.add(new double[] {x, y});
	}
	
	/** An example of how to render the track to a Graphics context */
	public void drawtrack(Graphics g) {
		
		g.setColor(new Color(0xff0000));

		for (Polygon p : polygons) {
			// g.drawLine((int) p.xpoints[0], (int) p.ypoints[0], (int)
			// p.xpoints[1],
			// (int) p.ypoints[1]);
			// g.drawLine((int) p.xpoints[2], (int) p.ypoints[2], (int)
			// p.xpoints[3],
			// (int) p.ypoints[3]);
			g.drawPolygon(p);
		}
		
		for (double[] p : getSpline().points) {
			g.fillOval((int)p[0], (int)p[1], 5, 5);
		}
	}
	
	/**
	 * Scans angle and distance between the last three controls points ending with index.
	 * 
	 * @param x
	 * 			x-coordinate of new control point
	 * @param y
	 * 			y-coordinate of new control point
	 */
	private boolean sanitise(double x, double y) {
		int last = this.spline.points.size() - 1;
		if (last < 1)
			return true;
		
		// Return false if last three control points form too sharp an angle.
		double[] p1 = this.spline.points.get(last - 1);
		double[] p2 = this.spline.points.get(last);
		double[] v1 = new double[2];
		double[] v2 = new double[2];
		
		// Calculate stuff
		Vector2D.vector(v1, p1[0], p1[1], p2[0], p2[1]);
		Vector2D.vector(v2, p2[0], p2[1], x, y);
		
		
		double length = Vector2D.magnitude(v2);
		
		// Control point has to be at least half track width away
		if (length < this.halfwidth)
			return false;
		
		// Calculate angle formed by last three control points
		double angle = Math.acos(Vector2D.dot(v1, v2) / (Vector2D.magnitude(v1)*Vector2D.magnitude(v2)))  * 180 / Math.PI;
		
		// Calculate max angle allowed
		double intervaldist = length / this.interval;
		double maxangle = ((Math.PI / 2) - Math.atan(intervaldist / this.halfwidth)) * 180 / Math.PI;
		
		return true;
	}
	
	/**
	 * Updates the curves and polygons after a change has been made.
	 */
	public void update() {
		// Don't do anything if there are not enough control points
		if (this.spline.points.size() < 4) return;
		// Else repeatedly call internal layout method until its happy with layout
		layout();
	}
	
	private int layout() {
		// Generate the curves intervals
		this.midcurve = this.spline.intervals(this.interval);
		
		Polygon polygon;
		double[][] left = new double[2][];
		double[][] right = new double[2][];
		this.leftcurve = new ArrayList<double[]>();
		this.rightcurve = new ArrayList<double[]>();
		double[][] p = new double[5][2];
    	int last = this.midcurve.size() - 1;
		
		// Clear the current list of polygons
		this.polygons.clear();
		
		// Project the first left and right curve interval
		p[1] = this.midcurve.get(0);
		p[2] = this.midcurve.get(1);
		Vector2D.vector(p[3], p[1][0], p[1][1], p[2][0], p[2][1]);
		projectEdges(p, left, right);
		left[1] = left[0];
		right[1] = right[0];
		
		// Project the left and right curve intervals
		for (int i = 1; i < last; i++) {
			p[0] = this.midcurve.get(i - 1);
			p[1] = this.midcurve.get(i);
    		p[2] = this.midcurve.get(i + 1);
    		Vector2D.vector(p[3], p[0][0], p[0][1], p[1][0], p[1][1]);
        	Vector2D.vector(p[4], p[1][0], p[1][1], p[2][0], p[2][1]);
        	Vector2D.add(p[3], p[4]);
    		projectEdges(p, left, right);
    		
    		//if (intervalsIntersect(left, right)) {
    			// Work out which control point we're closest to
    		//	int closest = (int)Math.round((i / this.interval)) + 1;
    		//	straightenCorner(closest);
    		//	return i;
    		//}

			// Add the polygon
			polygon = new Polygon();
			polygon.addPoint((int) left[0][0], (int) left[0][1]);
			polygon.addPoint((int) left[1][0], (int) left[1][1]);
			polygon.addPoint((int) right[1][0], (int) right[1][1]);
			polygon.addPoint((int) right[0][0], (int) right[0][1]);
			this.polygons.add(polygon);

			// Remember current left/right points
			left[1] = left[0];
			right[1] = right[0];
		}
		
		
		// Project the last left and right curve interval
		p[0] = this.midcurve.get(last - 1);
		p[1] = this.midcurve.get(last);
		Vector2D.vector(p[3], p[0][0], p[0][1], p[1][0], p[1][1]);
		projectEdges(p, left, right);
		
		// Add the last polygon
		polygon = new Polygon();
		polygon.addPoint((int) left[0][0], (int) left[0][1]);
		polygon.addPoint((int) left[1][0], (int) left[1][1]);
		polygon.addPoint((int) right[1][0], (int) right[1][1]);
		polygon.addPoint((int) right[0][0], (int) right[0][1]);
		this.polygons.add(polygon);
		
		return -1;
	}
	
	/**
	 * Used by update() to project the points on the left and right edges of the track.
	 * 
	 * @param p
	 * 			Point array
	 * @param left
	 * 			Left points array
	 * @param right
	 * 			Right points array
	 */
	private void projectEdges(double[][] p, double[][] left, double[][] right) {
		Vector2D.rotate90(p[3]);
		Vector2D.setLength(p[3], this.halfwidth);
		left[0] = new double[]{p[1][0]+p[3][0], p[1][1]+p[3][1]};
		this.leftcurve.add(left[0]);
		right[0] = new double[]{p[1][0]-p[3][0], p[1][1]-p[3][1]};
		this.rightcurve.add(right[0]);
	}
	
	/**
	 * Used by update() to test for interval intersection.
	 * 
	 * @param left
	 * 			Left points array
	 * @param right
	 * 			Right points array
	 * @return
	 * 			
	 */
	private boolean intervalsIntersect(double[][] left, double[][] right) {
		double z1, z2;
		int s1, s2;
		
		if ((z1 = ((left[0][0]-left[1][0])*(right[1][1]-left[1][1])) - ((left[0][1]-left[1][1])*(right[1][0]-left[1][0]))) < 0)
			s1 = -1;
		else if (z1 > 0)
			s1 = 1;
		else
			s1 = 0;
		
		if ((z2 = ((right[0][0]-left[1][0])*(right[1][1]-left[1][1])) - ((right[0][1]-left[1][1])*(right[1][0]-left[1][0]))) < 0)
			s2 = -1;
		else if (z2 > 0)
			s2 = 1;
		else
			s2 = 0;
		
		if ((s1 == 0 || s2 == 0) || s1 != s2)
			return true;
		
		return false;
	}

	private void straightenCorner(int index) {
		double[][] p = new double[5][2];
		p[0] = this.spline.points.get(index - 1);
		p[1] = this.spline.points.get(index);
		p[2] = this.spline.points.get(index + 1);
		Vector2D.vector(p[3], p[1][0], p[1][1], p[0][0], p[0][1]);
    	Vector2D.vector(p[4], p[1][0], p[1][1], p[2][0], p[2][1]);
    	Vector2D.add(p[3], p[4]);
    	Vector2D.setLength(p[3], this.halfwidth / 2);                
    	p[1][0] += p[3][0];
    	p[1][1] += p[3][1];
	}
}